// Multi Diversity FPV Receiver - Version 0.9.2 (31. May 2011)
//
// Visit <http://www.michael-heck.net and  <http://http://modellfluginfo.de/Modellflugzeug/Blog/fpv-diversity-empfaenger.php for detailed instruction
//
// Copyright (c) by Achim Bartelt <http://modellfluginfo.de> and Michael Heck <http://www.michael-heck.net>
// 
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
#include <LiquidCrystal.h>
#include <Servo.h> 
 
Servo myservo;  // create servo object to control a servo 

 
int rssi1 = A0;
int rssi2 = A1;
int rssi3 = A2;
int rssi4 = A3;
int antenne2zaehler=0;
int rueckwaerts=0;
int fastadjust=0;

int rssiValue1 = 0;
int rssiValue2 = 0;
int rssiValue3 = 0;
int rssiValue4 = 0;  

LiquidCrystal lcd(7, 6, 5, 4, 3, 2);

void setup() {
  
  lcd.begin(16, 2);
  myservo.attach(10);  // attaches the servo on pin 10 to the servo object 
  myservo.write(90);
  
  pinMode(8, OUTPUT);     
  pinMode(9, OUTPUT);    

  Serial.begin(9600);  
  lcd.setCursor(0, 0);
  lcd.print("michael-heck.net");
  lcd.setCursor(0, 1);
  lcd.print("Quad-Diversity");
  delay(3000); 
}

void loop(){
   lcd.clear();
  // Map values to defined range
  rssiValue1 = map(analogRead(rssi1), 0, 239, 0, 100);
  rssiValue2 = map(analogRead(rssi2), 0, 239, 0, 100);
  //rssiValue3 = map(analogRead(rssi3), 0, 239, 0, 100);
  //rssiValue4 = map(analogRead(rssi4), 0, 239, 0, 100);
  rssiValue3 = 0;
  rssiValue4 = 0;
  
   
  //Info to searial port
  Serial.println(rssiValue1);   
  Serial.println(rssiValue2);  
  Serial.println(rssiValue3);  
  Serial.println(rssiValue4);    
  Serial.println(" ");      
  
  //Display rssi values
  lcd.setCursor(0, 0);
  lcd.print(rssiValue1);
  lcd.setCursor(4, 0);
  lcd.print(rssiValue2);
  lcd.setCursor(8, 0);
  lcd.print(rssiValue3);
  lcd.setCursor(12, 0);
  lcd.print(rssiValue4); 
  if(rssiValue1>70 || rssiValue2>70 || rssiValue3>70 || rssiValue4>70) 
  {
  lcd.setCursor(0, 1);
  lcd.print("RX: SEHR GUT");
  goto bailout;
  }
  if(rssiValue1>60 || rssiValue2>60 || rssiValue3>60 || rssiValue4>60) 
  {
  lcd.setCursor(0, 1);
  lcd.print("RX: GUT");
  goto bailout;
  }
  if(rssiValue1>50 || rssiValue2>50 || rssiValue3>50 || rssiValue4>50) 
  {
  lcd.setCursor(0, 1);
  lcd.print("RX: OK");
  goto bailout;
  }
  if(rssiValue1>40 || rssiValue2>40 || rssiValue3>40 || rssiValue4>40) 
  {
  lcd.setCursor(0, 1);
  lcd.print("RX: AUSREICHEND");
  goto bailout;
  }
  if(rssiValue1>30 || rssiValue2>30 || rssiValue3>30 || rssiValue4>30) 
  {
  lcd.setCursor(0, 1);
  lcd.print("RX: SCHLECHT");
  goto bailout;
  }
  if(rssiValue1<10 || rssiValue2<10 || rssiValue3<10 || rssiValue4<10) 
  {
  lcd.setCursor(0, 1);
  lcd.print("RX: KEIN SIGNAL");
  goto bailout;
  }

bailout:
  
  //Select receiver with best rssi value and display selected channel
  if(rssiValue1 > rssiValue2 && rssiValue1 > rssiValue3 && rssiValue1 > rssiValue4){
    digitalWrite(8, LOW);
    digitalWrite(9, LOW); 
    lcd.setCursor(3, 0);
    lcd.print("<");    
    lcd.setCursor(7, 0);
    lcd.print(" ");  
    lcd.setCursor(11, 0);
    lcd.print(" ");  
    lcd.setCursor(15, 0);
    lcd.print(" ");      
  }  
  else if(rssiValue2 > rssiValue1 && rssiValue2 > rssiValue3 && rssiValue2 > rssiValue4){
    digitalWrite(8, HIGH);
    digitalWrite(9, LOW); 
    lcd.setCursor(3, 0);
    lcd.print(" ");    
    lcd.setCursor(7, 0);
    lcd.print("<");  
    lcd.setCursor(11, 0);
    lcd.print(" ");  
    lcd.setCursor(15, 0);
    lcd.print(" ");  
  }
  else if(rssiValue3 > rssiValue1 && rssiValue3 > rssiValue2 && rssiValue3 > rssiValue4){
    digitalWrite(8, LOW);
    digitalWrite(9, HIGH);
    lcd.setCursor(3, 0);
    lcd.print(" ");    
    lcd.setCursor(7, 0);
    lcd.print(" ");  
    lcd.setCursor(11, 0);
    lcd.print("<");  
    lcd.setCursor(15, 0);
    lcd.print(" ");     
  }  
  else if(rssiValue4 > rssiValue1 && rssiValue4 > rssiValue2 && rssiValue4 > rssiValue3){
    digitalWrite(8, HIGH);
    digitalWrite(9, HIGH); 
    lcd.setCursor(3, 0);
    lcd.print(" ");    
    lcd.setCursor(7, 0);
    lcd.print(" ");  
    lcd.setCursor(11, 0);
    lcd.print(" ");  
    lcd.setCursor(15, 0);
    lcd.print("<");        
  }    
  
  //Servo adjusting Antenna 2
  if(rssiValue2 < 50){
      rssiValue2 = map(analogRead(A1), 0, 239, 0, 100);
      myservo.write(antenne2zaehler);
      delay(50); 
      lcd.setCursor(0, 1);
      lcd.print("Ant. 2 ausrichten...");
      
      if (antenne2zaehler>=180)
      {
        rueckwaerts=1;
      }
      if (antenne2zaehler<=1)
      {
        rueckwaerts=0; 
      }
      if (rueckwaerts==0)
      {
        antenne2zaehler=antenne2zaehler+1;
      }
      if (rueckwaerts==1)
      {
        antenne2zaehler=antenne2zaehler-1;
      }
      
  }
  delay(100);  
}

